package com.hitqz.robot.biz.flow.component.oilSample;

import com.baomidou.mybatisplus.core.toolkit.Wrappers;
import com.hitqz.robot.api.business.enums.BrandEnum;
import com.hitqz.robot.api.business.enums.EquipmentEnum;
import com.hitqz.robot.api.common.dto.robot.RobotMoveDto;
import com.hitqz.robot.api.common.entity.RobotEquipment;
import com.hitqz.robot.biz.service.RobotEquipmentService;
import com.hitqz.robot.camera.EquipmentFactory;
import com.hitqz.robot.camera.IEquipmentService;
import com.hitqz.robot.camera.dto.EquipmentParamsDto;
import com.hitqz.robot.camera.dto.QueryParamsDto;
import com.hitqz.robot.driver.parent.RobotDriverService;
import com.yomahub.liteflow.annotation.LiteflowComponent;
import com.yomahub.liteflow.core.NodeComponent;
import lombok.RequiredArgsConstructor;
import lombok.extern.slf4j.Slf4j;

import java.util.List;
import java.util.Optional;
import java.util.concurrent.TimeUnit;

/**
 * @author xupkun
 * @date 2024/11/14
 */
@LiteflowComponent(value = "ForwardAdjustComponent", name = "[取油样机器人]前进后退调整")
@RequiredArgsConstructor
@Slf4j
public class ForwardAdjustComponent extends NodeComponent {


    private final RobotEquipmentService robotEquipmentService;

    private final RobotDriverService robotDriverService;

    private final EquipmentFactory equipmentFactory;



    @Override
    public void process() throws Exception {

        TimeUnit.SECONDS.sleep(3);
        List<RobotEquipment> oils = robotEquipmentService.list(Wrappers.<RobotEquipment>lambdaQuery()
                .eq(RobotEquipment::getType, EquipmentEnum.PLC.getType())
                .eq(RobotEquipment::getBrand, BrandEnum.CAR_PLC.getEnStr())
                .eq(RobotEquipment::getIsRecordParam, 1));

        RobotEquipment carPlc = oils.get(0);

        IEquipmentService service = equipmentFactory.getServiceByBrand(carPlc.getBrand());
        QueryParamsDto queryParamsDto = new QueryParamsDto();
        queryParamsDto.setPassword(carPlc.getPassword());
        queryParamsDto.setUsername(carPlc.getUsername());
        queryParamsDto.setPort(carPlc.getPort());
        queryParamsDto.setChannel(carPlc.getChannel());
        queryParamsDto.setIp(carPlc.getIp());

        while (true) {
            TimeUnit.SECONDS.sleep(1);
            List<EquipmentParamsDto> equipmentParamsDtos = service.getEquipmentParams(queryParamsDto);
            Optional<EquipmentParamsDto> leftDistance = equipmentParamsDtos.stream().filter(t->t.getKey().equals("leftDistance")).findFirst();
            Optional<EquipmentParamsDto> rightDistance = equipmentParamsDtos.stream().filter(t->t.getKey().equals("rightDistance")).findFirst();
            if (leftDistance.isPresent() && rightDistance.isPresent()) {
                Integer left = Integer.parseInt(leftDistance.get().getValue());
                Integer right = Integer.parseInt(rightDistance.get().getValue());
                log.info("left distance {} --- right distance {}, {} ", left, right, Math.abs(Math.abs(left) - Math.abs(right)));
                if (Math.abs(Math.abs(left) - Math.abs(right)) < 8)  {
                    log.info("左右相等{}-{}", left, right);
                    break;
                }
                String speed = "0.02";
                if (right.compareTo(left) > 0) {
                    speed = "-0.02";
                }
                RobotMoveDto robotMoveDto = new RobotMoveDto();
                robotMoveDto.setLinerSpeed(speed);
                robotMoveDto.setLinerSpeedY("0");
                for (int i = 0; i < 50; i ++) {
                    robotDriverService.setSpeed(robotMoveDto);
                }
            }
        }


    }
}
